#include "Openmv.h"
#include "Bujin.h"
#include "board.h"
#include "Control.h"
#include <stdint.h>
#include <stdio.h>

// 宏定义
#define PACKET_LEN   6
#define HEAD_BYTE    0xEE
#define TAIL_BYTE    0xFE
#define OFFSET       32768  // 偏移量

// 接收缓冲与变量
uint8_t uart_rx_buf[PACKET_LEN] = {0};
uint8_t uart_rx_cnt = 0;
uint8_t uart_data_ready = 0;

volatile int16_t data1 = 0;  // X误差（有符号）
volatile int16_t data2 = 0;  // Y误差（有符号）
volatile int Rectanglse_Flag = 0; // 0=未检测，1=检测到

// 串口发送单字符
void uart1_send_char(char ch) {
    while (DL_UART_isBusy(OpenMV_INST));
    DL_UART_Main_transmitData(OpenMV_INST, ch);
}

// 串口发送字符串
void uart1_send_string(char* str) {
    while (*str != 0 && str != NULL) {
        uart1_send_char(*str++);
    }
}

// 串口中断服务函数
void OpenMV_INST_IRQHandler(void) {
    uint8_t uart_data;

    switch (DL_UART_getPendingInterrupt(OpenMV_INST)) {
        case DL_UART_IIDX_RX:
            uart_data = DL_UART_Main_receiveData(OpenMV_INST);

            if (uart_rx_cnt == 0) {
                if (uart_data == HEAD_BYTE) {
                    uart_rx_buf[uart_rx_cnt++] = uart_data;
                }
            } else if (uart_rx_cnt < PACKET_LEN - 1) {
                uart_rx_buf[uart_rx_cnt++] = uart_data;
            } else {
                if (uart_data == TAIL_BYTE) {
                    uart_rx_buf[uart_rx_cnt] = uart_data;
                    uart_data_ready = 1;

                    // 判断是否为“未检测到矩形”的特殊数据
                    if (uart_rx_buf[1] == 0xFF && uart_rx_buf[2] == 0xFF &&
                        uart_rx_buf[3] == 0xFF && uart_rx_buf[4] == 0xFF) {
                        Rectanglse_Flag = 0;
                        data1 = 0;
                        data2 = 0;
                    } else {
                        uint16_t raw_data1 = ((uint16_t)uart_rx_buf[1] << 8) | uart_rx_buf[2];
                        uint16_t raw_data2 = ((uint16_t)uart_rx_buf[3] << 8) | uart_rx_buf[4];

                        data1 = (int16_t)(raw_data1 - OFFSET);
                        data2 = (int16_t)(raw_data2 - OFFSET);
                        Rectanglse_Flag = 1;

                    }
                }

                // 重置计数器，无论包是否有效
                uart_rx_cnt = 0;
            }
            break;

        default:
            break;
    }
}



//步进启动默认位置
void BujinStart()
{
    int L_PWM,R_PWM=0;
    L_PWM=Calculate_MA_Angle_PWM(10);
    R_PWM=Calculate_MB_Angle_PWM(10);
    Set_PWM(L_PWM,R_PWM);//PWM波驱动电机
}

//开始寻找矩形
void Find_Rectangle(void)
{
    int L_PWM,R_PWM=0;
    if(Rectanglse_Flag == 0) //没有找到
    {   
        R_PWM=Calculate_target(30); 
        Set_PWM(0,R_PWM);//PWM波驱动电机
    }
    else if(Rectanglse_Flag ==1)//找到矩形
    {
        //
    }
}
